clear all;
close all;
dirMS = 'H:\EXPData\Invivo\20191017\H1\Analysis\MST1';

dirMS = uigetdir('H:\EXPData\Invivo\','Please select the root dir of ImgsDCM');

dcmPath =  [dirMS filesep 'imgsDCM'];
dirInfo = dir(dcmPath);

Tsat = 600; %ms
sliceNum = 9;
imgNum = 10;
RRnum = 2;
sliceImgandInfo=cell(sliceNum,1);
acqTimeArr = zeros(sliceNum,imgNum);
imgArrs = [];
acqDate = 20190822*1000000;
for ix = 3: length(dirInfo)
    sliceDirName = dirInfo(ix,1).name;
    sliceDirPatch = [dcmPath filesep sliceDirName];
    s = dicomLoadAllSeries(sliceDirPatch, '', 'true',1);
    for ij = 1:length(s)
        acqTimeArr(ix-3+1,ij) =convertHMStoS(s(ij).AcquisitionDateTime-acqDate);
        imgArrs(:,:,ix-3+1,ij) = s(ij).imData;
    end
    sliceImgandInfo{ix-3+1,1} = s;
end


[ value, acqOrder] = sort(acqTimeArr(:));
TsatArr = zeros( sliceNum, imgNum);
TinvArr= zeros( sliceNum, imgNum);
scanTm_s = value(end)-value(1);

difftm = diff([ acqTimeArr(acqOrder(1))' acqTimeArr(acqOrder(1:90))']);

useNavTmforCorrection = 1;

if(useNavTmforCorrection)
    
    [navInfo Tsat] = ReadNavfromRawData();
    NavTMStamp = navInfo.timestamp;
    
    diffValNavTMStamp= diff([NavTMStamp(1) NavTMStamp] )*2.5;
    difftm = diff([ acqTimeArr(acqOrder(1))' acqTimeArr(acqOrder(1:90))']);
    figure, hold on; plot(difftm*1000); plot(diffValNavTMStamp,'r'); hold off;
    
    validNavNavLen =( sliceNum +RRnum)*imgNum;
    valNavTMStamp = NavTMStamp( (length(NavTMStamp) - validNavNavLen):end-1 );
    diffValNavTMStamp= diff([valNavTMStamp(1) valNavTMStamp] )*2.5;
    figure, hold on; plot(difftm*1000); plot(diffValNavTMStamp,'r'); hold off;
    
    
    
    loopofoneShot = sliceNum+RRnum ;
    arrdiffValNavTMStamp = reshape( diffValNavTMStamp, [loopofoneShot imgNum ]);
    
    
    
    imgNavTM = zeros(sliceNum, imgNum);
    
    
    for ix = 1:imgNum
        for ij =1:sliceNum
            if(ij<5)
                imgNavTM (ij,ix) = arrdiffValNavTMStamp(ij, ix);
            elseif(ij==5)
                imgNavTM (ij, ix) = sum(arrdiffValNavTMStamp(ij+[1:RRnum],ix));
            else
                imgNavTM (ij,ix) = arrdiffValNavTMStamp(ij+RRnum,ix);
            end
        end
    end
    
    imgNavTM = imgNavTM(:);
    figure, hold on; plot(difftm*1000); plot(imgNavTM,'r'); hold off;
end

for ix = 1:imgNum
    rrArrs  =[];
    for ij =1:sliceNum
        if( ix==1)
            TsatArr(acqOrder(ij))  = eps;
            TinvArr(acqOrder(ij))  = Inf;
        else
            if(ij<5)
                TsatArr(acqOrder(ij+(ix-1)*sliceNum))  = Tsat;
                TinvArr(acqOrder(ij+(ix-1)*sliceNum))  = (acqTimeArr(acqOrder(ij+(ix-1)*sliceNum))-acqTimeArr(acqOrder(1+(ix-1)*sliceNum)))*1000;
                if(TinvArr(acqOrder(ij+(ix-1)*sliceNum)) >0  )
                    rrArrs = [rrArrs (acqTimeArr(acqOrder(ij+(ix-1)*sliceNum))-acqTimeArr(acqOrder(ij-1+(ix-1)*sliceNum)))*1000];
                end
            else
                if(useNavTmforCorrection)
                    TsatArr(acqOrder(ij+(ix-1)*sliceNum))  = Tsat+imgNavTM(5+(ix-1)*sliceNum);   % don't use mean(rrArrs) * RRnum.
                    TinvArr(acqOrder(ij+(ix-1)*sliceNum))  = (acqTimeArr(acqOrder(ij+(ix-1)*sliceNum))-acqTimeArr(acqOrder(5+(ix-1)*sliceNum)))*1000;
                else
                    TsatArr(acqOrder(ij+(ix-1)*sliceNum))  = Tsat+ mean(rrArrs) * RRnum;
                    TinvArr(acqOrder(ij+(ix-1)*sliceNum))  = (acqTimeArr(acqOrder(ij+(ix-1)*sliceNum))-acqTimeArr(acqOrder(5+(ix-1)*sliceNum)))*1000;
                end
            end
        end
    end
end


%resotre image and recover time
[ value, initSliceorder] = sort(min(acqTimeArr,[],2));

initSliceOrder = initSliceorder';
scanOrderofAllslices = zeros(imgNum,sliceNum);
scanOrderofAllslicesArr = [];
for ix = 1:10
    scanOrderofAllslices(ix,:) = initSliceOrder;
    scanOrderofAllslicesArr= [scanOrderofAllslicesArr initSliceOrder];
    initSliceOrder =mod( [initSliceOrder+1], 9);
    initSliceOrder(initSliceOrder==0) = 9;
end

dirMat = [dirMS filesep 'ImgsMat' ];
mkdir([dirMat]);
newOrder=[];
for ix  =1:sliceNum
    [pos] = find(scanOrderofAllslicesArr==ix);
    imgDat = squeeze( imgArrs(:,:,acqOrder(pos)));
    TinvTM = squeeze( TinvArr(acqOrder(pos)));
    TsatTM = squeeze( TsatArr(acqOrder(pos)));
    
    % restore the image as seq order
    IMG(:,:,1) = squeeze(imgDat(:,:,1));
    Info.Tinv(1) = squeeze(TinvTM(1));
    Info.Tsat(1) = squeeze(TsatTM(1));
    for ij = 2: imgNum
        [v, cols] = find(scanOrderofAllslices(ij,:)==ix);
        newOrder(ij) = cols;
        IMG(:,:,cols+1) =  imgDat(:,:, ij ) ;
        Info.Tinv(cols+1) =   squeeze(TinvTM(ij));
        Info.Tsat(cols+1) = squeeze(TsatTM(ij));
        Info.FailedMocoBit = zeros(1,size(IMG,3));
    end
    Info.scanTm_s = scanTm_s;
    %%save
    sliceName =[dirMat filesep 'slice' num2str(ix) '.mat' ];
    save(sliceName, 'IMG', 'Info');
end


function tm_S =  convertHMStoS(tm)

HMSDat = fix(tm);
HM = fix(HMSDat/100);
H = fix(HM/100);

msecond =  tm-HMSDat;
second =(HMSDat/100-HM)*100;
secondM = ((HM/100 - H))*100*60;
secondH =  H*3600;


tm_S=secondH+secondM+second+msecond;

end

